#pragma once

#include <boost/thread.hpp>
#include <chrono>
#include "modules/AssistModule/LogModule.h"

namespace behavior_controller
{

    class FrequencyBase
    {
    public:
        FrequencyBase(const std::function<void()> &_callback) : callback_(_callback) {}
        virtual ~FrequencyBase() = default;

        void Start()
        {
            callback_thread_ = boost::thread(&FrequencyBase::Run, this);
        }
        void Stop()
        {
            callback_thread_.interrupt();
            callback_thread_.join();
        }

    private:
        void Run()
        {
            ResetClock();
            while (true)
            {
                boost::this_thread::interruption_point();
                callback_();
                boost::this_thread::interruption_point();
                WaitForNextCycle();
            }
        }

        virtual void ResetClock() = 0;
        virtual void WaitForNextCycle() = 0;

        std::function<void()> callback_;
        boost::thread callback_thread_;
    };

    template <typename Clock>
    class Frequency : public FrequencyBase
    {
    public:
        typedef typename Clock::TimePoint TimePoint;
        typedef typename Clock::Duration Duration;

        Frequency(const std::function<void()> &_callback)
            : FrequencyBase(_callback), start_time_point_(Clock::Now()), period_(0)
        {
        }

        void SetPeriod(const Duration &_period) { period_ = _period; }

        void SetFrequency(double _frequency)
        {
            typename Duration::rep rep =
                Duration::period::den / Duration::period::num / _frequency;
            period_ = Duration(rep);
        }

    private:
        TimePoint start_time_point_;
        Duration period_;

        void ResetClock() override { start_time_point_ = Clock::Now(); }

        void WaitForNextCycle() override
        {
            TimePoint expect_time_point = start_time_point_ + period_;
            TimePoint current_time_point = Clock::Now();

            if (current_time_point < start_time_point_)
            {
                expect_time_point = current_time_point + period_;
            }

            TimePoint end_time_point_ =
                std::max(current_time_point, expect_time_point);

            Clock::SleepUntil(end_time_point_);

            start_time_point_ = end_time_point_;
        }
    };

} // namespace behavior_controller
